#define WTCON		0x53000000	/* Watchdog timer control register */
#define INTMSK		0x4A000008	/* Interrupt Mask register */
#define INTSUBMSK	0x4A00001C	/* Sub Interrupt Mask register */

#define TASK_STK_SIZE	1024		/* stack size */

#define USR_MOD		0x10
#define FIQ_MOD		0x11
#define IRQ_MOD		0x12
#define SVC_MOD		0x13
#define ABT_MOD		0x17
#define UND_MOD		0x1b
#define SYS_MOD		0x1F

#define MSK_MOD		0x1F

#define NO_IRQ 		0x80
#define NO_FIQ 		0x40
#define NO_INT 		0xC0

.globl _start
_start:
	b       start_code
	ldr	pc, _undefined_instruction
	ldr	pc, _software_interrupt
	ldr	pc, _prefetch_abort
	ldr	pc, _data_abort
	ldr	pc, _not_used
	ldr	pc, _irq
	ldr	pc, _fiq

	_undefined_instruction:	.word undefined_instruction
	_software_interrupt:	.word software_interrupt
	_prefetch_abort:	.word prefetch_abort
	_data_abort:		.word data_abort
	_not_used:		.word not_used
	_irq:			.word irq
	_fiq:			.word fiq

_start_kernel:
	.word start_kernel
_TEXT_BASE:
	.word TEXT_BASE

/* the actual start code */
start_code:
	bl	set_misc
	bl	relocate_vec_table
	bl	set_stack

	ldr	pc, _start_kernel

set_misc:
	/* set the cpu to SVC32 mode and disable irq/fiq */
	mrs	r0, cpsr
	bic	r0, r0, #0x1f
	orr	r0, r0, #0xd3
	msr	cpsr, r0

	/* turn off the watchdog */
	ldr     r0, =WTCON
	mov     r1, #0x0
	str     r1, [r0]

	/* mask all IRQs by setting all bits in the INTMSK */
	ldr	r1, =0xffffffff
	ldr	r0, =INTMSK
	str	r1, [r0]
	ldr	r1, =0xffff
	ldr	r0, =INTSUBMSK
	str	r1, [r0]

	mov	pc, lr

relocate_vec_table:
	/*
	 * The reason Why we need to relocate the vector table is because the
	 * image is running at address _TEXT_BASE but according to arm manuali:
	 * The vector table should be in 0x0000-0000
	 */
	mov	r0, #0
	ldr	r1, _TEXT_BASE
	mov	r2, #0x40
vec_cpy_next:
	ldr	r3, [r1], #4
	str	r3, [r0], #4
	subs	r2, r2, #4
	bne	vec_cpy_next

	mov	pc, lr
set_stack:
	/* Set up the stack */

	/* SVC stack */
	mov	r0, #TASK_STK_SIZE
	sub	r0, r0, #1
	mov	r1, r0, LSL #2
	ldr	r0, =SvcStk
	add	r2, r0, r1
	mov	sp, r2

	/* IRQ stack */
	msr	cpsr_c, #NO_INT|IRQ_MOD
	mov	r0, #TASK_STK_SIZE
	sub	r0, r0, #1
	mov	r1, r0, LSL #2
	ldr	r0, =IrqStk
	add	r2, r0, r1
	mov	sp, r2

	/* FIQ stack */
	msr	cpsr_c, #NO_INT|FIQ_MOD
	mov	r0, #TASK_STK_SIZE
	sub	r0, r0, #1
	mov	r1, r0, LSL #2
	ldr	r0, =FiqStk
	add	r2, r0, r1
	mov	sp, r2

	msr	cpsr_c, #NO_INT|SVC_MOD
	mov	pc, lr
#if 0
	ldr	r0, =CONFIG_MM_BASE
	sub	r0, r0, #CONFIG_MALLOC_LEN		/* malloc area */
	ldr	r1, =MALLOC_START
	str	r0, [r1]

clear_bss:
	ldr	r0, _bss_start		/* find start of bss segment        */
	ldr	r1, _bss_end		/* stop here                        */
	mov 	r2, #0x00000000		/* clear                            */

clbss_l:
	str	r2, [r0]		/* clear loop...                    */
	add	r0, r0, #4
	cmp	r0, r1
	ble	clbss_l
#endif

software_interrupt:
	b	OSCtxSw

irq:
	/*
	 * save r0, r1, r2 first, we'll use these 3 registers to store r14,
	 * spsr, sp(irq)
	 */
	stmfd   sp!, {r0-r2}
	sub	r0, lr, #4		/* pc				    */
	mrs	r1, spsr		/* spsr				    */
	mov	r2, sp			/* sp(irq)			    */
	add	sp, sp, #12

	msr	cpsr_c, #NO_INT|SVC_MOD

	/* save processor registers */
	stmfd	sp!, {r0}		/* r15, pc			    */
	stmfd   sp!, {r14}		/* r14, lr			    */
	stmfd   sp!, {r3-r12}		/* r3 - r12			    */

	ldmfd	r2, {r4-r6}		/* get r0, r1, r2		    */
	stmfd	sp!, {r4-r6}		/* push r0, r1, r2		    */

	stmfd	sp!, {r1}		/* get spsr			    */

	/* Save the current task's sp into the current task's TCB	    */
	ldr	r4, =OSTCBCur
	ldr	r5, [r4]
	str	sp, [r5, #0]

	bl	OSIntEnter
	msr	cpsr_c, #NO_INT|IRQ_MOD
	bl	do_irq
	msr	cpsr_c, #NO_INT|SVC_MOD
	bl	OSIntExit

	/* restore all processor registers, get the sp first*/
	ldr	r4, =OSTCBCur
	ldr	r5, [r4]
	ldr	sp, [r5, #0]

	ldmfd	sp!, {r4}
	msr	spsr_cxsf, r4

	ldmfd	sp!, {r0-r12, lr, pc}^

undefined_instruction:
prefetch_abort:
data_abort:
not_used:
fiq:
	b	_start

.globl hang  
hang:
	b	_start
